﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.ExportToRTR.CApExportToRTRRobotInstance
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using DnProcessSimulateCommands.ExportToRTR.YAML;
using EngineeringInternalExtension;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using Tecnomatix.Engineering;


namespace DnProcessSimulateCommands.ExportToRTR{

public class CApExportToRTRRobotInstance
{
  private TxRobot m_robot;
  private RobotPrototype m_robotPrototype;
  private TxReportWriterEx m_logWriter;
  private const string DefaultPreset = "robot_default_preset";

  public CApExportToRTRRobotInstance(
    TxRobot robot,
    RobotPrototype robotPrototype,
    TxReportWriterEx logWriter)
  {
    this.m_robot = robot;
    this.m_robotPrototype = robotPrototype;
    this.m_logWriter = logWriter;
  }

  public RobotInstance Export()
  {
    RobotInstance yamlRobot = this.ExportRobotInstance();
    this.ExportRobotFrame(this.m_robot.Toolframe);
    Frame yamlTCPF = this.ExportRobotFrame(this.m_robot.TCPF);
    this.SetRobotDefaultPreset(yamlRobot, yamlTCPF);
    this.ExportRobotMate(yamlRobot);
    this.ExportSoftLimits(yamlRobot);
    return yamlRobot;
  }

  private void ExportRobotMate(RobotInstance yamlRobot)
  {
    Mate toReferenceFrame = CApExportToRTRYamlMgr.Instance.CreateMateToReferenceFrame((ITxLocatableObject) this.m_robot, (IObjectInstance) yamlRobot);
    yamlRobot.Mate = toReferenceFrame;
  }

  private void ExportSoftLimits(RobotInstance yamlRobot)
  {
    foreach (ITxObject drivingJoint in (Collection<ITxObject>) this.m_robot.DrivingJoints)
    {
      IObj robotKinematicChild = CApExportToRTRYamlMgr.Instance.GetRobotKinematicChild(yamlRobot.RobotPrototype, drivingJoint);
      double? nullable = (robotKinematicChild as Joint).Limits.MinPosition;
      if (nullable.HasValue)
      {
        nullable = (robotKinematicChild as Joint).Limits.MaxPosition;
        if (nullable.HasValue)
          yamlRobot.JointLimits.Add(new JointSoftLimit()
          {
            Joint = drivingJoint.Name,
            MinPosition = new double?((drivingJoint as TxJoint).LowerSoftLimit),
            MaxPosition = new double?((drivingJoint as TxJoint).UpperSoftLimit)
          });
      }
    }
  }

  private RobotInstance ExportRobotInstance()
  {
    RobotInstance yamlObj = new RobotInstance()
    {
      Name = TxObjectEx.GetUniqueNameForRTR((ITxObject) this.m_robot, false),
      Model = this.m_robotPrototype.Name,
      RobotPrototype = this.m_robotPrototype
    };
    CApExportToRTRYamlMgr.Instance.AddObj<RobotInstance>(yamlObj, (ITxObject) this.m_robot);
    this.m_robot.SetInstanceParameter((TxRoboticParam) new TxRoboticStringParam("OLP_RTR_PROJECT_NAME", TxObjectEx.GetUniqueNameForRTR((ITxObject) this.m_robot, false)));
    return yamlObj;
  }

  private void SetRobotDefaultPreset(RobotInstance yamlRobot, Frame yamlTCPF)
  {
    Preset preset = new Preset()
    {
      Name = "robot_default_preset",
      TcpFrame = yamlTCPF.Name
    };
    yamlRobot.DefaultPreset = preset.Name;
    yamlRobot.Presets = new List<Preset>() { preset };
  }

  private ITxLocatableObject GetFrameParent(TxFrame frame)
  {
    return frame.AttachmentParent != null ? frame.AttachmentParent : frame.Collection as ITxLocatableObject;
  }

  private Frame ExportRobotFrame(TxFrame frame)
  {
    ITxLocatableObject frameParent = this.GetFrameParent(frame);
    TxTransformation origin = CApExportToRTRRobotSupplement.GetOrigin((ITxLocatableObject) frame, frameParent, (ITxObjectCollection) this.m_robot);
    Frame yamlObj = new Frame()
    {
      Name = TxObjectEx.GetUniqueNameForRTR((ITxObject) frame, false),
      Parent = CApExportToRTRRobotSupplement.CreateParentObj(frameParent, (ITxObjectCollection) this.m_robot),
      OffsetXyz = new Vec3D(origin.Translation),
      OffsetRpy = new Vec3D(origin.GetRotation())
    };
    CApExportToRTRYamlMgr.Instance.AddObj<Frame>(yamlObj, (ITxObject) frame);
    return yamlObj;
  }

  internal static TxKinematicLink GetLastLinkInMainChain(TxRobot robot)
  {
    return (((Collection<ITxObject>) robot.DrivingJoints)[((Collection<ITxObject>) robot.DrivingJoints).Count - 1] as TxJoint).ChildLink;
  }
}
